import os
from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch.substitutions import Command
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    
    ros2_video_node = Node(
            package='ros2_video',
            executable='ros2_video_node',
            parameters=[os.path.join(
                get_package_share_directory('ros2_video'), 'config', 'node_params.yaml'),
                        {'camera_info_url': 'package://ros2_video/config/camera_info.yaml'}],
            output='screen',
            emulate_tty=True,
            remappings=[
            ('/image_raw', '/camera/image_raw'),
            ('/camera_info', '/camera/camera_info')
    ])
    
    # 获取URDF文件的路径
    base_description = Command(['xacro ', os.path.join(
        get_package_share_directory('controller_tracker'), 'urdf', 'controller_base.urdf.xacro')])
    

    
    # 创建robot_state_publisher节点

    base_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': base_description,
                    'publish_frequency': 1000.0}])


    # 创建ComposableNode列表
    composable_nodes = [
        ComposableNode(
            package='image_proc',
            plugin='image_proc::RectifyNode',
            name='rectify',
            namespace='camera',
            remappings=[
                ('/camera/image', '/camera/image_raw'),
                ('/camera/camera_info', '/camera/camera_info')
            ]
        ),
        ComposableNode(
            package='apriltag_ros',
            plugin='AprilTagNode',
            name='apriltag',
            namespace='apriltag',
            parameters=[os.path.join(
                get_package_share_directory('apriltag_ros'), 'cfg', 'tags_36h11.yaml')],
            remappings=[
                ('/apriltag/image_rect', '/camera/image_rect'),
                ('/apriltag/camera_info', '/camera/camera_info')
            ]
        ),
        ComposableNode(
            package='controller_tracker',
            plugin='controller_tracker::ControllerTrackerNode',
            name='controller_tracker',
            namespace='controller_tracker',
            parameters=[os.path.join(
                get_package_share_directory('controller_tracker'), 'config', 'tags_positions.yaml')],
            remappings=[
                ('/controller_tracker/detections', '/apriltag/detections')
            ]
        )
    ]

    # 创建ComposableNodeContainer
    container = ComposableNodeContainer(
        name='controller_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=composable_nodes
    )

    return LaunchDescription([
        base_state_publisher_node,
        # target_state_publisher_node,
        container, 
        ros2_video_node
        ])
